#include <stdio.h>
#include "VEXRobot.h"
#include "ifi_picdefs.h"
#include "ifi_aliases.h"
#include <UserAPI.h>
#include <adc.h>
#include "ifi_utilities.h"

// Buffer to hold the IO port associated with an ultrasonic sensor
char ultrasonicIOPort[NUM_INTERUPTS];

// Number of ADC channels
char numAdc;

// IO control
char ioControl0;
char ioControl1;

// Bitmask of which interrupts are actually ultrasonic sensors
char sonarMask;

// Bitmask of which interrupts are actually encoders
char encoderMask;

/*
** Set the IO configuration
*/
void SetConfiguration(char *buff)
{
   int   i;
   char  changeEncoderMask, changeSonarMask;
   char  changeMask;

   // Get the IO direction data
   ioControl0 = buff[0];
   ioControl1 = buff[1];

   // Get the number of ADC ports
   numAdc = buff[2];

   // Get the encoder ports used (and changed)
   changeEncoderMask = encoderMask ^ buff[3];
   encoderMask = buff[3];

   // Get the sonar ports used (and changed)
   changeSonarMask = sonarMask ^ buff[4];
   sonarMask = buff[4];

   // If the port changed type to/from encoder,
   // start or stop the encoder as appropriate
   // (Remember that the ports are 1 based)
   changeMask = 1;
   for(i=0; i<NUM_INTERUPTS; ++i)
   {
      if(changeEncoderMask & changeMask)
      {
         if(encoderMask & changeMask)
         {
            StartEncoder(i+1);
            PresetEncoder(i+1, 0);
         } else {
            StopEncoder(i+1);
         }
      }
      changeMask <<= 1;
   }

   // If the port changed type to/from ultrasonic,
   // start or stop the device as appropriate
   // (Remember that the ports are 1 based)
   changeMask = 1;
   for(i=0; i<NUM_INTERUPTS; ++i)
   {
      if(changeSonarMask & changeMask)
      {
         if(sonarMask & changeMask)
         {
            ultrasonicIOPort[i] = buff[5 + i];
         }
      }
      changeMask <<= 1;
   }

   // Set the direction of each pin
   IO1 = (ioControl0 & 0x01) ? INPUT : OUTPUT;
   IO2 = (ioControl0 & 0x02) ? INPUT : OUTPUT;
   IO3 = (ioControl0 & 0x04) ? INPUT : OUTPUT;
   IO4 = (ioControl0 & 0x08) ? INPUT : OUTPUT;
   IO5 = (ioControl0 & 0x10) ? INPUT : OUTPUT;
   IO6 = (ioControl0 & 0x20) ? INPUT : OUTPUT;
   IO7 = (ioControl0 & 0x40) ? INPUT : OUTPUT;
   IO8 = (ioControl0 & 0x80) ? INPUT : OUTPUT;
   IO9 = (ioControl1 & 0x01) ? INPUT : OUTPUT;
   IO10 = (ioControl1 & 0x02) ? INPUT : OUTPUT;
   IO11 = (ioControl1 & 0x04) ? INPUT : OUTPUT;
   IO12 = (ioControl1 & 0x08) ? INPUT : OUTPUT;
   IO13 = (ioControl1 & 0x10) ? INPUT : OUTPUT;
   IO14 = (ioControl1 & 0x20) ? INPUT : OUTPUT;
   IO15 = (ioControl1 & 0x40) ? INPUT : OUTPUT;
   IO16 = (ioControl1 & 0x80) ? INPUT : OUTPUT;

   Set_Number_of_Analog_Channels(numAdc);
}

//
// Set all of the motors
//
void SetAllMotors(char *buff)
{
#ifdef RECIEVE_DEBUG
   printf("SetAllMotors %d %d %d %d %d %d %d %d\n",
      buff[0], buff[1], buff[2], buff[3], 
      buff[4], buff[5], buff[6], buff[7]);
#endif

   // Set the new values
   SetPWM( 1, buff[0]);
   SetPWM( 2, buff[1]);
   SetPWM( 3, buff[2]);
   SetPWM( 4, buff[3]);
   SetPWM( 5, buff[4]);
   SetPWM( 6, buff[5]);
   SetPWM( 7, buff[6]);
   SetPWM( 8, buff[7]);
}

//
// Get the input values to the controller
//
void GetAllInput(char *buff)
{
   // Set the ADC port values as the first bytes
   // Since they have to be consecutive starting at
   // port 1, this simplifies the logic
   char  i;
   for(i=0; i<numAdc; ++i)
   {
      SetResultShort( i*2, adcValues[i]);
   }

   // Fill in the rest of the IO port values with
   // the current digital values.  (Note that output
   // values read as whatever the user set it to.)
   for( ; i<NUM_IO_PORTS; ++i)
   {
      // printf("%d - %d\n", i, GetDigitalInput(i + 1) ? 0xffff : 0 );
      SetResultShort( i*2, GetDigitalInput(i + 1) ? 0xffff : 0 );
   }

   // The ultrasonic and encoder values are set in the background.
   // Copy them into the buffer.
   for(i=0; i<NUM_INTERUPTS; ++i)
   {
      SetResultLong( NUM_IO_PORTS * sizeof(short) + i * sizeof(long), interruptValues[i]);
   }
}

/*
** Set the digital output ports
*/
void SetAllPorts(char *buff)
{
   int   i;
   char  mask = 1;
   for(i=0; i<8; ++i, mask <<= 1)
   {
      if(0 == (ioControl0 & mask))
      {
         SetDigitalOutput(i+1, (buff[0] & mask) ? 1 : 0);
      }
   }

   mask = 1;
   for(i=8; i<NUM_IO_PORTS; ++i, mask <<= 1)
   {
      if(0 == (ioControl1 & mask))
      {
         SetDigitalOutput(i+1, (buff[1] & mask) ? 1 : 0);
      }
   }
}

/*
** Preset the encoder value
*/
void EncoderPreset(char *buff)
{
   if(buff[0] > 0 && buff[0] <= NUM_INTERUPTS)
   {
      PresetEncoder(buff[0], * ((long *) &buff[1]));
   }
}
